#include "Polygon3D.h"

using namespace libPortal;
using namespace std;
using namespace Eigen;

Polygon3D::Polygon3D(Eigen::Hyperplane<double, 3> p, double prec) :
  plane(p),
  maxErr(abs(prec))
{
  Vector3d n(p.normal());
  Vector3d up(0,1,0);
  Vector3d axis(n.cross(up));
  double rads = 0;
  if(axis.norm()>maxErr) {
    axis.normalize();
    rads = acos(n.dot(up));
  } else 
    axis = Vector3d(1,0,0);
  AngleAxisd aa(rads, axis);
  polyTrans = Transform<double, 3>(aa);
}

void Polygon3D::cut(Eigen::Hyperplane<double, 3> plane)
{
  assert(isValid());
  
  //transform the cut plane into polygon storage space (y = 0)
  {
    //plane.transform(polyTrans.inverse());  
    Vector3d n(plane.normal());
    double d = plane.offset();
    Matrix4d inv(polyTrans.inverse());
    Vector4d normal4;
    normal4[0] = n[0];
    normal4[1] = n[1];
    normal4[2] = n[2];
    normal4[3] = 0;
    Vector4d pointInPlane(normal4*d);
    pointInPlane = inv*pointInPlane;
    normal4 = inv*normal4;
    n[0] = normal4[0];
    n[1] = normal4[1];
    n[2] = normal4[2];
    n.normalize();
    Vector3d p;
    p[0] = pointInPlane[0];
    p[1] = pointInPlane[1];
    p[2] = pointInPlane[2];
    plane = Hyperplane<double, 3>(n, p);
  }

  Hyperplane<double, 2> cutLine;

  //compute the line in the y = 0 plane that cuts the polygon...
  {
    //http://mathworld.wolfram.com/Plane-PlaneIntersection.html
    //halfway down that page.
    Vector3d n1(0,1,0);
    Vector3d n2(plane.normal());
    double p1 = 0;
    double p2 = plane.offset();
    Matrix<double, 2,3> m;
    m.row(0) = n1;
    m.row(1) = n2;
    Vector2d p(p1,p2);
    LU<Matrix<double, 2, 3> > lu(m);
    Vector3d linePt;
    if(!lu.solve(-p, &linePt))
      return;  //cut plane doesn't intersect, do nothing.
    Matrix<double, 3, 1> dir;
    lu.computeKernel(&dir);
    dir.normalize();
    assert(abs(linePt[1])<1e-6);
    cutLine = Hyperplane<double, 2>::Through(Vector2d(linePt[0], linePt[2])+Vector2d(dir[0], dir[2]),
					     Vector2d(linePt[0], linePt[2]));
    cout<<"lineDir: "<<dir<<endl;
    cout<<"linePt: "<<linePt<<endl;
    //return;
  }

  //add verts where the plane intersects the edges
  vertex_iter iter, end, last;
  iter = points.begin();
  end = points.end();
  last = end;
  --last;
  do {
    ParametrizedLine<double, 2> polyEdge;
    Vector2d edgeVector(*iter-*last);
    double lineLength = edgeVector.norm();
    edgeVector /= lineLength;
    polyEdge = ParametrizedLine<double, 2>(*last, edgeVector); //origin, dir
    double inter = polyEdge.intersection(cutLine);
    if(inter>maxErr && inter<lineLength-maxErr) {
      //line intersect, cut it.
      Vector2d intPoint(polyEdge.direction()*inter+*last);
      points.insert(iter, intPoint);
    }
    last = iter;
    ++iter;
  } while(iter!=end);
  
  //remove all verts outside the plane.
  for(iter=begin();iter!=end;++iter) {
    if(cutLine.signedDistance(*iter)>maxErr) {
      iter = points.erase(iter);
      --iter;
    }
  }
}

bool Polygon3D::isValid() const
{
  unsigned int numberOfPoints = numVerts();
  if(numberOfPoints==0)
    return true;
  if(numberOfPoints<3)
    return false;
  
  const_vertex_iter iter, end;
  end = Polygon3D::end();

  //make sure there are no edge intersections.
  iter = begin();
  const_vertex_iter last = end;
  --last;
  Vector2d testA, testB;
  testA = *iter;
  testB = *last;
  while(iter!=end) {
    const_vertex_iter testStart = iter;
    const_vertex_iter testEnd;
    testEnd = testStart;
    ++testEnd;
    while(testEnd!=end) {
      Vector2d A, B;
      A = *testStart;
      B = *testEnd;
      //Intersect (A,B) and (testA, testB)
      {
	Vector2d ab(B-A);
	double lenab = ab.norm();
	Vector2d abN = ab;
	abN.normalize();
	Vector2d tab(testB-testA);
	double lentab = tab.norm();
	Vector2d tabN = tab;
	tabN.normalize();
	ParametrizedLine<double, 2> lineab(A,abN);
	ParametrizedLine<double, 2> linetab(testA,tabN);
	if(lineab.isApprox(linetab, maxErr))
	  return false;
	Hyperplane<double, 2> planeab(Hyperplane<double, 2>::Through(A,B));
	Hyperplane<double, 2> planetab(Hyperplane<double, 2>::Through(testA,testB));
	double abINT = lineab.intersection(planetab);
	double tabINT = linetab.intersection(planeab);
	if(abINT>maxErr && abINT+maxErr<lenab &&
	   tabINT>maxErr && tabINT+maxErr<lentab)
	  return false;
      }
      ++testStart;
      ++testEnd;
    }
    testB = *iter;
    ++iter;
    testA = *iter;
  }
  return true;
}

bool Polygon3D::isConvex() const
{
  assert(numVerts()>=3);
  const const_vertex_iter end(Polygon3D::end());
  bool dir = false;
  {
    const_vertex_iter last(end);
    --last;
    Vector2d a(*last);
    Vector2d b(*begin());
    Vector2d c(*(++begin()));
    Vector2d ba(a-b);
    Vector2d bc(c-b);
    double sign(Vector3d(ba[0], 0.0, ba[1]).cross(Vector3d(bc[0], 0.0, bc[1]))[1]);
    if(sign<0)
      dir = true;
  }

  const_vertex_iter aIter, bIter, cIter;
  aIter = bIter = cIter = begin();
  advance(bIter, 1);
  advance(cIter, 2);

  do {
    Vector2d a(*aIter);
    Vector2d b(*bIter);
    Vector2d c(*cIter);
    Vector2d ba(a-b);
    Vector2d bc(c-b);
    double sign(Vector3d(ba[0], 0.0, ba[1]).cross(Vector3d(bc[0], 0.0, bc[1]))[1]);
    if(dir && sign>0)
      return false;
    else if(!dir && sign<0)
      return false;
    ++aIter;
    ++bIter;
    ++cIter;
  } while(cIter!=end);

  {
    const_vertex_iter last(end);
    --last;
    Vector2d b(*last);
    --last;
    Vector2d a(*last);
    Vector2d c(*begin());
    Vector2d ba(a-b);
    Vector2d bc(c-b);
    double sign(Vector3d(ba[0], 0.0, ba[1]).cross(Vector3d(bc[0], 0.0, bc[1]))[1]);
    if(dir && sign>0)
      return false;
    else if(!dir && sign<0)
      return false;        
  }
  return true;
}

double Polygon3D::calculateArea() const
{
  return 0.0;
}

bool Polygon3D::intersects(const Polygon3D &other) const
{
  return false;
}

const_vertex_iter Polygon3D::begin() const
{
  return points.begin();
}


vertex_iter Polygon3D::begin()
{
  return points.begin();
}


const_vertex_iter Polygon3D::end() const
{
  return points.end();
}


vertex_iter Polygon3D::end()
{
  return points.end();
}


bool Polygon3D::append(Eigen::Vector3d p)
{
  const Matrix4d invT(polyTrans.inverse());
  Vector4d pPoint(p[0], p[1], p[2], 1.0);
  pPoint = invT*pPoint;
  if(abs(pPoint[1])>maxErr)
    return false;
  points.push_back(Vector2d(pPoint[0], pPoint[2]));
  return true;
}

void Polygon3D::setMaxQuad()
{
  points.clear();
  points.push_back(Vector2d(-numeric_limits<double>::max(), -numeric_limits<double>::max()));
  points.push_back(Vector2d(-numeric_limits<double>::max(), numeric_limits<double>::max()));
  points.push_back(Vector2d(numeric_limits<double>::max(), numeric_limits<double>::max()));
  points.push_back(Vector2d(numeric_limits<double>::max(), -numeric_limits<double>::max()));
}

unsigned int Polygon3D::numVerts() const
{
  return points.size();
}

void Polygon3D::convexTriangleDecomposition(std::vector<Eigen::Vector4d> &verts, 
					    bool flower) const
{
  verts.clear();

  //----------------"Flower" pattern decomposition------------//
  //Advantage: Nice evenly spaced triangles
  //Disadvantage: More triangles.
  
  if(numVerts()<=4) //never take a center for 3-4 verts.
    {
      const_vertex_iter iter, end;
      end = Polygon3D::end();
      for(iter = begin();iter!=end;++iter) {
	Vector4d v((*iter)[0], 0.0, (*iter)[1], 1.0);
	v = polyTrans*v;
	verts.push_back(v);
      }
      return;
    }
  else if(flower)
    {
      Vector2d flatAve(0.0, 0.0);
      Vector4d worldAve;
      Vector4d a, b;
      //Average out all the positions of all the verts.
      const_vertex_iter iter, end;
      end = Polygon3D::end();
      for(iter=begin();iter!=end;iter++)
	flatAve += (*iter);
      flatAve /= ((double)numVerts());
      worldAve = Vector4d(flatAve[0], 0.0, flatAve[1], 1.0);
      worldAve = polyTrans*worldAve;  
      verts.push_back(worldAve);
    }

  //----------------------Fan pattern decomposition----------//
  //Advantage: fewer polygons, but streched out.
  //---------------------------------------------------------//
  const_vertex_iter iter, end;
  end = Polygon3D::end();  
  for(iter = begin();iter!=end;iter++) {
    Vector4d v((*iter)[0], 0.0, (*iter)[1], 1.0);
    v = polyTrans*v;
    verts.push_back(v);
  }
  //and the first one last time, so that rendering will draw the last "leaf"
  //if the flower pattern.
  if(flower)
    verts.push_back(verts[1]);
}

void Polygon3D::triangleDecomposition(std::vector<Eigen::Vector4d> &verts, bool flower) const
{
  if(points.size()==0) {
    verts.clear();
    return;
  }
  assert(isValid());
  if(isConvex())
    convexTriangleDecomposition(verts, flower);
  else
    concaveTriangleDecomposition(verts);
}

void Polygon3D::concaveTriangleDecomposition(vector<Vector4d> &verts) const
{
  assert(0);
}
